#include "BluetoothTask.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "usart.h"
#include "GlobalVars.h"

#define BT_BUFFER_SIZE 128

static char bt_buffer[BT_BUFFER_SIZE];
static int bt_buffer_index = 0;
static bool data_ready = false;

void BluetoothTask(void const *argument) {
    while (1) {
        if (data_ready) {
            processReceivedData(bt_buffer);
            data_ready = false;
            bt_buffer_index = 0;
        }
        osDelay(10);  // 小延迟，避免占用太多CPU时间
    }
}

void UARTReceiveTask(void const *argument) {
    uint8_t c;
    while (1) {
        if (HAL_UART_Receive(&huart2, &c, 1, 10) == HAL_OK) {
            if (bt_buffer_index < BT_BUFFER_SIZE - 1) {
                bt_buffer[bt_buffer_index++] = c;
                if (c == '#') {
                    bt_buffer[bt_buffer_index] = '\0';
                    data_ready = true;
                }
            } else {
                bt_buffer_index = 0;  // 防止缓冲区溢出
            }
        }
        osDelay(1);  // 小延迟，避免占用太多CPU时间
    }
}


int parseCommand(const char *buffer, Command *cmd) {
    char *ptr = (char *) buffer + 1;  // 跳过 '$'
    cmd->cmd1 = strtol(ptr, &ptr, 10);
    ptr++;
    cmd->cmd2 = strtol(ptr, &ptr, 10);
    ptr++;
    cmd->cmd3 = strtol(ptr, &ptr, 10);
    ptr++;
    cmd->cmd4 = strtol(ptr, &ptr, 10);
    ptr++;
    cmd->cmd6 = strtol(ptr, &ptr, 10);
    ptr++;
    cmd->data8 = strtol(ptr, &ptr, 10);
    ptr++;
    cmd->data9 = strtol(ptr, &ptr, 10);
    ptr++;
    cmd->data10 = strtol(ptr, &ptr, 10);
    ptr++;
    cmd->data11 = strtol(ptr, &ptr, 10);
    return (*ptr == '#') ? 0 : -1;
}


void executeCommand(const Command *cmd) {
    fireSensor = 0;
    if (cmd->data11) {
        servo_state = Middle_SERVO;
    } else if (cmd->data10) {
        fireSensor = 1;
    } else if (cmd->cmd6) {
        servo_state = cmd->cmd6;
    } else if (cmd->cmd2) {
        if (cmd->cmd2 == 1) {
            movement_state = 5;
        } else if (cmd->cmd2 == 2) {
            movement_state = 6;
        } else {
            movement_state = 0;
        }
    } else {
        switch (cmd->cmd1) {
            case 1:
                movement_state = 1;
                // Car_Run(2000);
                break;
            case 2:
                movement_state = 2;
                // Car_Back(1500);
                break;
            case 3:
                movement_state = 3;
                // Car_Left(2000);
                break;
            case 4:
                movement_state = 4;
                // Car_Right(2000);
                break;
            default:
                movement_state = 0;
                // Car_Stop();
                break;
        }
    }

//    HAL_Delay(10);
}

int parseModeCommand(const char *buffer) {
    if (strncmp(buffer, "\$4WD,MODE", 9) != 0 || buffer[strlen(buffer) - 1] != '#') {
        return -1;  // 命令格式错误
    }
    return atoi(buffer + 9);
}


void executeModeCommand(int modeValue) {
    RobotMode mode = modeValue / 10 * 10;
    int isOn = modeValue % 10;

    if (isOn && mode != currentMode) {
        currentMode = MODE_REMOTE;  // 如果关闭模式，默认回到遥控模式
    }

    switch (mode) {
        case MODE_REMOTE:
            if (isOn) {

                // 开启遥控模式的代码
                currentMode = MODE_REMOTE;
            }
            break;
        case MODE_LINE_FOLLOWING:
            if (isOn) {
                // 开启巡线模式的代码
                currentMode = MODE_LINE_FOLLOWING;
            }
            break;
        case MODE_OBSTACLE_AVOIDANCE:
            if (isOn) {
                // 开启避障模式的代码
                currentMode = MODE_OBSTACLE_AVOIDANCE;
            }
            break;
        case MODE_COLORFUL_LIGHT:
            if (isOn) {
                // 开启七彩探照模式的代码
                currentMode = MODE_COLORFUL_LIGHT;
            }
            break;
        case MODE_LIGHT_SEEKING:
            if (isOn) {
                // 开启寻光模式的代码
                currentMode = MODE_LIGHT_SEEKING;
            }
            break;
        case MODE_FOLLOWING:
            if (isOn) {
                // 开启跟随模式的代码
                currentMode = MODE_FOLLOWING;
            }
            break;
        default:
            // 未知模式
            break;
    }

    if (!isOn) {
        // 关闭当前模式
        switch (currentMode) {
//            case MODE_REMOTE:
//                // 关闭遥控模式的代码
//                break;
//            case MODE_LINE_FOLLOWING:
//                // 关闭巡线模式的代码
//                uint32_t start_time = HAL_GetTick();
//                while (HAL_GetTick() - start_time > 100) {
//                    movement_state = 0;
//                }
//                break;
//            case MODE_COLORFUL_LIGHT:
//                r = 0;
//                g = 0;
//                b = 0;
//                break;
//                // ... 其他模式的关闭代码
            default:
                break;
        }
        currentMode = MODE_REMOTE;  // 如果关闭模式，默认回到遥控模式
    }
}

void processReceivedData(const char *data) {
    if (strstr(data, "\$4WD,MODE") != NULL) {
        int modeValue = parseModeCommand(data);
        if (modeValue != -1) {
            executeModeCommand(modeValue);
        }
    } else {
        Command cmd;
        if (parseCommand(data, &cmd) == 0) {
            executeCommand(&cmd);
        }
    }
}
